#include "Dog.h"

/* 相位计算 */
static inline void calcContactPhase()
{
    dog.period = global.throttle;
    for (int i = 0; i < 4; i++)
    {
        /* 计算时间偏差 */
        float normalT = fmod(global.runTime + dog.period * global.bias[i], dog.period) / dog.period;

        if (normalT < dog.stRatio) // 站立态
        {
            global.contact[i] = 1;
            global.phase[i] = normalT / dog.stRatio;
        }
        else // 摆动态
        {
            global.contact[i] = 0;
            global.phase[i] = (normalT - dog.stRatio) / dog.swRatio;
        }
    }
}

/**
 * 狗子初始化
 * 只执行一遍
 */
void Dog_Init(void)
{
    UART_Print("Program start\n");

    /* 因为每条腿的前后半腿不一样, 所以需要初始化腿的编号并分配前后半腿的电机 */
    Leg_Init(&dog.legs[0], 0, &dog.motors[0], &dog.motors[1]);
    Leg_Init(&dog.legs[1], 1, &dog.motors[3], &dog.motors[2]);
    Leg_Init(&dog.legs[2], 2, &dog.motors[5], &dog.motors[4]);
    Leg_Init(&dog.legs[3], 3, &dog.motors[6], &dog.motors[7]);

#if OPEN_CAN
    /* 初始化CAN和电机ID */
    CAN_FilterInit();
    AttachInterrupt_CAN(&hcan1, HT_CAN_Callback);
    AttachInterrupt_CAN(&hcan2, HT_CAN_Callback);
    HT_Init(&dog.motors[0], 1, &hcan2);
    HT_Init(&dog.motors[1], 2, &hcan2);
    HT_Init(&dog.motors[2], 3, &hcan1);
    HT_Init(&dog.motors[3], 4, &hcan1);
    HT_Init(&dog.motors[4], 5, &hcan1);
    HT_Init(&dog.motors[5], 6, &hcan1);
    HT_Init(&dog.motors[6], 7, &hcan2);
    HT_Init(&dog.motors[7], 8, &hcan2);
#endif

#if REMOTE
    /* 初始化遥控 */
    Remote_Init();
#endif

    /* 初始化有限状态机 */
    FSM_Init(PASSIVE);

    global.height = 240; // 身体高度300

    /* 初始化完成 */
    UART_Print("Initialization complete\n");
}

/* 设置周期 */
inline void Dog_SetPeriod(float period, float stanceRatio)
{
    dog.period = period;
    dog.stRatio = stanceRatio;
    dog.swRatio = 1 - dog.stRatio;
}

/**
 * 狗子运行
 * 需要循环执行
 */
inline void Dog_Run(void)
{
    /* 获得运行时间 ms */
    global.runTime = __HAL_TIM_GetCounter(&GLOBAL_TIM) / 1000.0f;

    /* 如果正在走，则计算摆线 */
    if (global.isWalking)
    {
        /* 根据当前时间计算相位 */
        calcContactPhase();
        for (uint8_t i = 0; i < 4; i++)
        {
            /* 根据相位计算电机角度 */
            calcCycloid(&dog.legs[i]);
        }
    }

    /* 运行有限状态机, 执行当前状态函数, 设置电机参数 */
    FSM_Run();

    for (uint8_t i = 0; i < 4; i++)
    {
        /* 运行四条腿电机 */
        Leg_Run(&dog.legs[i]);
    }
}
